/**
* This file is part of LIO-mapping.
* 
* Copyright (C) 2019 Haoyang Ye <hy.ye at connect dot ust dot hk>,
* Robotics and Multiperception Lab (RAM-LAB <https://ram-lab.com>),
* The Hong Kong University of Science and Technology
* 
* For more information please see <https://ram-lab.com/file/hyye/lio-mapping>
* or <https://sites.google.com/view/lio-mapping>.
* If you use this code, please cite the respective publications as
* listed on the above websites.
* 
* LIO-mapping is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
* 
* LIO-mapping is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
* GNU General Public License for more details.
* 
* You should have received a copy of the GNU General Public License
* along with LIO-mapping.  If not, see <http://www.gnu.org/licenses/>.
*/

//
// Created by hyye on 4/4/18.
//

#include "factor/AutoPoseLocalParameterization.h"

namespace lio{

bool AutoPoseLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const
{
  Eigen::Map<const Eigen::Vector3d> p(x + 4);
  Eigen::Map<const Eigen::Quaterniond> q(x);

  Eigen::Map<const Eigen::Vector3d> dp(delta + 3);

  Eigen::Map<Eigen::Vector3d> p_plus(x_plus_delta + 4);
  Eigen::Map<Eigen::Quaterniond> q_plus(x_plus_delta);

  // const double norm_delta = sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
  // if (norm_delta > 0.0) {
  //   const double sin_delta_by_delta = sin(norm_delta) / norm_delta;
  //   // Note, in the constructor w is first.
  //   Eigen::Quaterniond delta_q(cos(norm_delta),
  //                              sin_delta_by_delta * delta[0],
  //                              sin_delta_by_delta * delta[1],
  //                              sin_delta_by_delta * delta[2]);
  //   q_plus = delta_q * q;
  // } else {
  //   q_plus = q;
  // }

  // delta[0] = delta[1] = 0; 旋转约束
  const double norm_delta = sqrt(delta[2] * delta[2]);
  if (norm_delta > 0.0) {
    const double sin_delta_by_delta = sin(norm_delta) / norm_delta;
    // Note, in the constructor w is first.
    Eigen::Quaterniond delta_q(cos(norm_delta),
                               0,
                               0,
                               sin_delta_by_delta * delta[2]);
    q_plus = delta_q * q;
  } else {
    q_plus = q;
  }

  p_plus = p + dp;

  return true;
}

bool AutoPoseLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const
{
  Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> j(jacobian);
  // j.topRows<6>().setIdentity();
  // j.bottomRows<1>().setZero();
  j.setZero();
  j.bottomRightCorner<3, 3>().setIdentity();
  jacobian[0] =  x[3];  jacobian[1]  =  x[2];  jacobian[2]  = -x[1];
  jacobian[6] = -x[2];  jacobian[7]  =  x[3];  jacobian[8]  =  x[0];
  jacobian[12] =  x[1];  jacobian[13]  = -x[0];  jacobian[14]  =  x[3];
  jacobian[18] = -x[0];  jacobian[19] = -x[1];  jacobian[20] = -x[2];
  Eigen::Matrix3d right_info_mat;
  right_info_mat.setZero();
  right_info_mat(0, 0) = 0;
  right_info_mat(1, 1) = 0;
  right_info_mat(2, 2) = 1;
  j.topLeftCorner<4, 3>() = j.topLeftCorner<4, 3>() * right_info_mat;
  // j的左上角4*3为q对theta的导数,右下角3*3为I,其余为0

  return true;
}

// 不带旋转约束
bool NoConstrainLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const
{
  Eigen::Map<const Eigen::Vector3d> p(x + 4);
  Eigen::Map<const Eigen::Quaterniond> q(x);

  Eigen::Map<const Eigen::Vector3d> dp(delta + 3);

  Eigen::Map<Eigen::Vector3d> p_plus(x_plus_delta + 4);
  Eigen::Map<Eigen::Quaterniond> q_plus(x_plus_delta);

  const double norm_delta = sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
  if (norm_delta > 0.0) {
    const double sin_delta_by_delta = sin(norm_delta) / norm_delta;
    // Note, in the constructor w is first.
    Eigen::Quaterniond delta_q(cos(norm_delta),
                               sin_delta_by_delta * delta[0],
                               sin_delta_by_delta * delta[1],
                               sin_delta_by_delta * delta[2]);
    q_plus = delta_q * q;
  } else {
    q_plus = q;
  }

  p_plus = p + dp;

  return true;
}

bool NoConstrainLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const
{
  Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> j(jacobian);
  // j.topRows<6>().setIdentity();
  // j.bottomRows<1>().setZero();
  j.setZero();
  j.bottomRightCorner<3, 3>().setIdentity();
  jacobian[0] =  x[3];  jacobian[1]  =  x[2];  jacobian[2]  = -x[1];
  jacobian[6] = -x[2];  jacobian[7]  =  x[3];  jacobian[8]  =  x[0];
  jacobian[12] =  x[1];  jacobian[13]  = -x[0];  jacobian[14]  =  x[3];
  jacobian[18] = -x[0];  jacobian[19] = -x[1];  jacobian[20] = -x[2];

  return true;
}

bool AnalyticPoseLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const
{
  Eigen::Map<const Eigen::Vector3d> p(x + 4);
  Eigen::Map<const Eigen::Quaterniond> q(x);

  Eigen::Map<const Eigen::Vector3d> dp(delta + 3);

  Eigen::Quaterniond dq = DeltaQ(Eigen::Map<const Eigen::Vector3d>(delta));

  Eigen::Map<Eigen::Vector3d> p_plus(x_plus_delta + 4);
  Eigen::Map<Eigen::Quaterniond> q_plus(x_plus_delta);

  p_plus = p + dp;
  q_plus = (dq * q).normalized();

  return true;
}

bool AnalyticPoseLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const
{
  Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> j(jacobian);
  j.topRows<6>().setIdentity();
  j.bottomRows<1>().setZero();

  return true;
}

}
